#include "bsp_mc_svpwm_3shunt.h"
#include "bsp_mc_timer.h"
#include "bsp_mc_adc.h"
#include "bsp_mc_drv8323.h"

#define SQRT_3		1.732051
#define T		    (PWM_PERIOD * 4)
#define T_SQRT3     (u16)(T * SQRT_3)

#define SECTOR_1	(u32)1
#define SECTOR_2	(u32)2
#define SECTOR_3	(u32)3
#define SECTOR_4	(u32)4
#define SECTOR_5	(u32)5
#define SECTOR_6	(u32)6
	




u8  bSector;  
u16  hTimePhA=PWM_PERIOD/2-1, hTimePhB=PWM_PERIOD/2-1, hTimePhC=PWM_PERIOD/2-1, hTimePhD=PWM_PERIOD -TSAMPLE;

Curr_Components SVPWM_3ShuntGetPhaseCurrentValues(void)
{
	 Curr_Components Local_Stator_Currents;
	 s32 wAux;
	 switch (bSector)
	 {
		 case 4: 
		 case 5:
				//Current on Phase C not accessible   
				wAux = ((ADC1->JDR1)<<4)-(s32)(hPhaseAOffset) ; 
				if (wAux < S16_MIN)
				{
					Local_Stator_Currents.qI_Component1= S16_MIN;
				}  
				else  if (wAux > S16_MAX)
						{ 
							Local_Stator_Currents.qI_Component1= S16_MAX;
						}
						else
						{
							Local_Stator_Currents.qI_Component1= wAux;

						}

				wAux = ((ADC2->JDR1)<<4)-(s32)(hPhaseBOffset);
			 // Saturation of Ib
				if (wAux < S16_MIN)
				{
					Local_Stator_Currents.qI_Component2= S16_MIN;
				}  
				else  if (wAux > S16_MAX)
						{ 
							Local_Stator_Currents.qI_Component2= S16_MAX;
						}
						else
						{
							Local_Stator_Currents.qI_Component2= wAux;
		
						}
			   break;
		 case 6:
		 case 1:  //Current on Phase A not accessible  
	 
				wAux = ((ADC1->JDR1)<<4)-(s32)(hPhaseBOffset);	//B		
				//Saturation of Ib 
				if (wAux < S16_MIN)
				{
					  Local_Stator_Currents.qI_Component2= S16_MIN;
				}  
				else  if (wAux > S16_MAX)
						{ 
							Local_Stator_Currents.qI_Component2= S16_MAX;
						}
						else
						{
							Local_Stator_Currents.qI_Component2= wAux;

						}
				// Ia = -Ic -Ib 
			    wAux = (s32)hPhaseCOffset-((ADC2->JDR1)<<4)-Local_Stator_Currents.qI_Component2;//A
				//Saturation of Ia
				if (wAux> S16_MAX)
				{
					 Local_Stator_Currents.qI_Component1 = S16_MAX;
				}
				else  if (wAux <S16_MIN)
					  {
						 Local_Stator_Currents.qI_Component1 = S16_MIN;
					  }
					  else
					  {  
						 Local_Stator_Currents.qI_Component1 = wAux;
	
					  }
				break;
						 
		 case 2:
		 case 3:  // Current on Phase B not accessible
             wAux = ((ADC1->JDR1)<<4)-(s32)(hPhaseAOffset);
            //Saturation of Ia 
            if (wAux < S16_MIN)
            {
                  Local_Stator_Currents.qI_Component1= S16_MIN;
            }  
            else  if (wAux > S16_MAX)
                  { 
                    Local_Stator_Currents.qI_Component1= S16_MAX;
                  }
                  else
                  {
                    Local_Stator_Currents.qI_Component1= wAux;

                  }
            // Ib = -Ic-Ia;
			
			 wAux = (s32)hPhaseCOffset-((ADC2->JDR1)<<4) - Local_Stator_Currents.qI_Component1;
            // Saturation of Ib
            if (wAux> S16_MAX)
            {
				  Local_Stator_Currents.qI_Component2=S16_MAX;
            }
            else  if (wAux <S16_MIN)
                  {  
                    Local_Stator_Currents.qI_Component2 = S16_MIN;
                  }
                  else  
                  {
                    Local_Stator_Currents.qI_Component2 = wAux;

                  }                     
           break;

		   default:
			   
           break;
	}

//	LED1_TOGGLE;//ADC interrup to here 1.4us
	return(Local_Stator_Currents); ;
}




void SVPWM_3ShuntCalcDutyCycles (Volt_Components Stat_Volt_Input)
{

   s32 wX, wY, wZ, wUAlpha, wUBeta;
  
   wUAlpha = Stat_Volt_Input.qV_Component1 * T_SQRT3 ;
   wUBeta = -(Stat_Volt_Input.qV_Component2 * T);

   wX = wUBeta;
   wY = (wUBeta + wUAlpha)/2;
   wZ = (wUBeta - wUAlpha)/2;


	// Sector calculation from wX, wY, wZ
   if (wY<0)
   {
	  if (wZ<0)
	  {
		bSector = SECTOR_5;
	  }
	  else // wZ >= 0
		if (wX<=0)
		{
		  bSector = SECTOR_4;
		}
		else // wX > 0
		{
		  bSector = SECTOR_3;
		}
   }
   else // wY > 0
   {
	 if (wZ>=0)
	 {
	   bSector = SECTOR_2;
	 }
	 else // wZ < 0
	   if (wX<=0)
	   {  
		 bSector = SECTOR_6;
	   }
	   else // wX > 0
	   {
		 bSector = SECTOR_1;
	   }
	}
  

  switch(bSector)
  {  
    case SECTOR_1:
								hTimePhA = (T/8) + ((((T + wX) - wZ)/2)/131072);
								hTimePhB = hTimePhA + wZ/131072;
								hTimePhC = hTimePhB - wX/131072;
								hTimePhD = PWM_PERIOD-TSAMPLE;
					
								ADC1->JSQR=PHASE_B_MSK;
								ADC2->JSQR=PHASE_C_MSK;             
                break;
		case SECTOR_2:
								hTimePhA = (T/8) + ((((T + wY) - wZ)/2)/131072);
								hTimePhB = hTimePhA + wZ/131072;
								hTimePhC = hTimePhA - wY/131072;
								
								hTimePhD = PWM_PERIOD-TSAMPLE;
						
								ADC1->JSQR=PHASE_A_MSK;
								ADC2->JSQR=PHASE_C_MSK;
				break;
		case SECTOR_3:
								hTimePhA = (T/8) + ((((T - wX) + wY)/2)/131072);
								hTimePhC = hTimePhA - wY/131072;
								hTimePhB = hTimePhC + wX/131072;

								hTimePhD = PWM_PERIOD-TSAMPLE;

								ADC1->JSQR=PHASE_A_MSK;
								ADC2->JSQR=PHASE_C_MSK;

                break;
    
    case SECTOR_4:
								hTimePhA = (T/8) + ((((T + wX) - wZ)/2)/131072);
								hTimePhB = hTimePhA + wZ/131072;
								hTimePhC = hTimePhB - wX/131072;
                
								hTimePhD = PWM_PERIOD-TSAMPLE;

								ADC1->JSQR=PHASE_A_MSK;
								ADC2->JSQR=PHASE_B_MSK;

                break;  
    
    case SECTOR_5:
								hTimePhA = (T/8) + ((((T + wY) - wZ)/2)/131072);
								hTimePhB = hTimePhA + wZ/131072;
								hTimePhC = hTimePhA - wY/131072;
                
								hTimePhD = PWM_PERIOD-TSAMPLE;

								ADC1->JSQR=PHASE_A_MSK;
								ADC2->JSQR=PHASE_B_MSK;

		break;
                
    case SECTOR_6:
								hTimePhA = (T/8) + ((((T - wX) + wY)/2)/131072);
								hTimePhC = hTimePhA - wY/131072;
								hTimePhB = hTimePhC + wX/131072;
												
								hTimePhD = PWM_PERIOD-TSAMPLE;

								ADC1->JSQR=PHASE_B_MSK;
								ADC2->JSQR=PHASE_C_MSK;
			
                break;
        default:
		        break;
	}



		/* Load compare registers values */ 
			MOTOR_TIM->CCR1 = hTimePhA;
			MOTOR_TIM->CCR2 = hTimePhB;
			MOTOR_TIM->CCR3 = hTimePhC;
			MOTOR_TIM->CCR4 = hTimePhD; // To Syncronyze the ADC

	
	
//			MOTOR_TIM->CCR1 = 2099;
//			MOTOR_TIM->CCR2 = 2099;
//			MOTOR_TIM->CCR3 = 2099;
//			MOTOR_TIM->CCR4 = PWM_PERIOD-TSAMPLE; // To Syncronyze the ADC
}


















